package com.intretech.tj.gpp.service

import com.intretech.tj.gpp.Graph
import com.intretech.tj.gpp.entity.DO.Edge
import com.intretech.tj.gpp.entity.DO.RfidNode
import com.intretech.tj.gpp.entity.DTO.EdgeStatusUpdate
import com.intretech.tj.gpp.entity.DTO.IDMParams
import com.intretech.tj.gpp.entity.DTO.PathRequest
import com.intretech.tj.gpp.entity.DTO.Vehicle.DynamicVehicleState
import com.intretech.tj.gpp.entity.DTO.Vehicle.VehicleMap
import com.intretech.tj.gpp.entity.DTO.Vehicle.VehicleStatus
import com.intretech.tj.gpp.entity.DTO.Vehicle.VehicleTaskInfo
import com.intretech.tj.gpp.entity.DTO.light.TrafficLightStatus
import com.intretech.tj.gpp.mapper.EdgeMapper
import com.intretech.tj.gpp.mapper.RfidMapper
import com.intretech.tj.gpp.message.*
import com.intretech.tj.gpp.pathfinding.Pathfinder
import com.intretech.tj.gpp.result.TaskResult
import io.quarkus.logging.Log
import io.vertx.core.impl.ConcurrentHashSet
import jakarta.enterprise.context.ApplicationScoped
import jakarta.inject.Inject
import org.slf4j.LoggerFactory
import java.util.*
import java.util.concurrent.ConcurrentHashMap
import java.util.concurrent.Executors
import java.util.concurrent.TimeUnit
import kotlin.math.max
import kotlin.math.pow
import kotlin.math.sqrt

/**
 * 全局路径规划(GPP)服务
 * 负责车辆路径规划、状态管理和路段控制等核心功能
 */
@ApplicationScoped
open class GPPService {

    @Inject
    private lateinit var edgeMapper: EdgeMapper // 查询边集

    @Inject
    private lateinit var rfidMapper: RfidMapper // 查询点集

    // 核心数据结构
    @Inject
    private lateinit var graph: Graph // 地图图结构

    @Inject
    private lateinit var pathfinder: Pathfinder// 路径查找器

    private val logger = LoggerFactory.getLogger(GPPService::class.java)


    // 并发安全的状态存储
    private val vehicleStates = ConcurrentHashMap<String, DynamicVehicleState>() // 车辆动态状态
    private val initializedVehicles = ConcurrentHashSet<String>() // 已初始化的车辆集合
    private val vehicleTaskMap = ConcurrentHashMap<String, VehicleTaskInfo>() // 车辆任务映射
    private val taskTopicToVehicleIdMap = ConcurrentHashMap<String, String>() // 任务Topic到车辆ID的映射

    // 任务调度器
    private val scheduler = Executors.newSingleThreadScheduledExecutor()

    companion object {
        private val GLOBAL_LOCK = Any() // 全局锁，用于同步操作
        private const val OFFLINE_TIMEOUT_MS = 5000 // 车辆离线超时时间(毫秒)
        private const val CLEANUP_INITIAL_DELAY_SEC = 10L // 清理任务初始延迟(秒)
        private const val CLEANUP_INTERVAL_SEC = 5L // 清理任务间隔(秒)
    }

    init {
        //启用离线车辆清理任务(目前注释掉，可根据需要启用)
        //startOfflineVehicleCleanupTask()
    }

    /**
     * 启动离线车辆清理任务
     * 定期检查并清理长时间未更新的车辆
     */
    private fun startOfflineVehicleCleanupTask() {
        scheduler.scheduleAtFixedRate(
            { cleanupOfflineVehicles() },
            CLEANUP_INITIAL_DELAY_SEC,
            CLEANUP_INTERVAL_SEC,
            TimeUnit.SECONDS
        )
    }

    /**
     * 清理离线车辆
     * 移除超过指定时间未更新的车辆
     */
    private fun cleanupOfflineVehicles() {
        try {
            val now = System.currentTimeMillis()

            val offlineVehicleIds = vehicleStates.entries
                .filter { (now - it.value.lastUpdateTime) > OFFLINE_TIMEOUT_MS }
                .map { it.key }

            if (offlineVehicleIds.isNotEmpty()) {
                logger.info("清理离线车辆: $offlineVehicleIds")
                for (vehicleId in offlineVehicleIds) {
                    // 从服务状态中移除
                    vehicleStates.remove(vehicleId)
                    // 从图的全局位置中移除
                    graph.removeVehicle(vehicleId)
                }
            }
        } catch (e: Exception) {
            logger.error("离线车辆清理失败", e)
        }
    }

    /**
     * 关闭服务，释放资源
     */
    fun shutdown() {
        scheduler.shutdown()
        try {
            if (!scheduler.awaitTermination(5, TimeUnit.SECONDS)) {
                scheduler.shutdownNow()
            }
        } catch (e: InterruptedException) {
            scheduler.shutdownNow()
            Thread.currentThread().interrupt()
        }
    }

    /**
     * 加载地图数据
     * @param nodes 节点列表
     * @param edges 边列表
     * @return 构建的车辆地图
     */
    private fun loadMap(nodes: List<RfidNode>, edges: List<Edge>): VehicleMap {
        graph.clear()
        nodes.forEach { graph.addNode(it) }
        edges.forEach { graph.addEdge(it) }
        return VehicleMap(graph.nodes, graph.edges)
    }

    /**
     * 车辆申请路径规划
     * @param request 路径请求
     * @return 规划的路径节点列表，如果无法规划则返回null
     */
    fun requestPath(request: PathRequest): List<Int>? {
        val result = pathfinder.findShortestPath(request.currentNodeId, request.endNodeId)

        result?.path?.let { path ->
            if (path.isEmpty()) {
                logger.warn("为车辆 ${request.vehicleId} 规划的路径为空")
                return null
            }

            // 初始化车辆的动态状态
            val initialState = DynamicVehicleState(
                vehicleId = request.vehicleId,
                currentAtEdge = null, // 刚开始时，车辆在起始节点，还未进入任何边
                currentSpeed = 0.0,
                edgeProgress = 0,
                batteryLife = 0,
                lastUpdateTime = System.currentTimeMillis(),
                plannedPath = path
            )

            Log.info("初始化车辆 ${request.vehicleId} 状态，时间: ${System.currentTimeMillis()}")
            vehicleStates[request.vehicleId] = initialState
        }

        return result?.path
    }

    /**
     * 获取指定车辆的动态状态
     * @param vehicleId 车辆ID
     * @return 车辆动态状态，如果不存在则返回null
     */
    private fun getVehicleProgress(vehicleId: String): DynamicVehicleState? {
        return vehicleStates[vehicleId]
    }

    /**
     * 获取所有车辆的状态信息
     * @return 车辆状态消息列表
     */
    fun getAllVehicle(): List<VehicleStatusMessage> {
        val vehicleStatusMessageList = mutableListOf<VehicleStatusMessage>()

        vehicleStates.forEach { (vehicleId, state) ->
            val currentNodeId: Int? = determineCurrentNodeId(vehicleId, state)

            val vehicleStatusMessage = VehicleStatusMessage(
                state.vehicleId,
                currentNodeId,
                state.currentSpeed,
                System.currentTimeMillis(),
                state.batteryLife,
                state.edgeProgress,
                state.currentAtEdge
            )
            vehicleStatusMessageList.add(vehicleStatusMessage)
        }

        return vehicleStatusMessageList
    }

    /**
     * 确定车辆当前所在的节点ID
     * @param vehicleId 车辆ID
     * @param state 车辆状态
     * @return 当前节点ID，如果无法确定则返回-1
     */
    private fun determineCurrentNodeId(vehicleId: String, state: DynamicVehicleState): Int? {
        return when {
            // 如果车辆当前正在某条边上，其最近的有效节点是该边的起点
            state.currentAtEdge != null -> {
                graph.getEdge(state.currentAtEdge!!)?.rfidNodeStart
            }
            // 如果车辆当前停留在某个节点上
            graph.vehicleAtNodeMap.containsKey(vehicleId) -> {
                graph.vehicleAtNodeMap[vehicleId]
            }

            else -> -1 // 无法确定位置
        }
    }

    /**
     * 更新车辆状态
     * 处理车辆位置和进度更新
     * @param status 车辆状态信息
     */
    private fun updateVehicleStatus(status: VehicleStatus) {
        val state = vehicleStates[status.vehicleId] ?: return

        // 修剪车辆路径, 直到开头为当前节点
        trimPlannedPathToCurrentNode(state, status.currentNodeId)

        // 检查车辆是否到达了其路径上的一个新节点
        val currentPathIndex = state.plannedPath.indexOf(status.currentNodeId)
        if (currentPathIndex == -1) {
            logger.warn("车辆 ${status.vehicleId} 上报的节点 ${status.currentNodeId} 不在规划路径上")
            return
        }

        val isAtDestination = currentPathIndex == state.plannedPath.lastIndex

        // 情况1: 车辆到达最终目的地
        if (isAtDestination) {
            handleVehicleAtDestination(status, state)
            return
        }

        // 情况2: 车辆在路径中间节点
        handleVehicleAtIntermediateNode(status, state, currentPathIndex)
    }


    /**
     * 处理车辆到达终点的情况
     */
    private fun handleVehicleAtDestination(status: VehicleStatus, state: DynamicVehicleState) {
        // 车辆停泊在终点节点
        graph.parkVehicleAtNode(status.vehicleId, status.currentNodeId)

        // 更新车辆状态
        state.currentAtEdge = null
        state.edgeProgress = 100
        state.lastUpdateTime = status.currentTime
        vehicleStates[status.vehicleId] = state

        logger.info("车辆 ${status.vehicleId} 已到达终点节点 ${status.currentNodeId}")
    }

    /**
     * 修剪车辆路径：将 plannedPath 的前缀节点移除，直到路径开头为 currentNodeId。
     * 如果 currentNodeId 不在路径中，则结果可能是空路径。
     */
    private fun trimPlannedPathToCurrentNode(state: DynamicVehicleState, currentNodeId: Int) {
        state.plannedPath = state.plannedPath.dropWhile { it != currentNodeId }
    }

    /**
     * 处理车辆在中间节点的情况
     */
    private fun handleVehicleAtIntermediateNode(
        status: VehicleStatus,
        state: DynamicVehicleState,
        currentPathIndex: Int
    ) {
        val nextNodeId = state.plannedPath[currentPathIndex + 1]
        val nextEdge = graph.findEdgeBetween(status.currentNodeId, nextNodeId)

        if (nextEdge != null && state.currentAtEdge != nextEdge.id) {
            // 车辆刚刚到达一个中间节点，准备进入下一条边
            graph.placeVehicleOnEdge(status.vehicleId, nextEdge.id)

            // 更新状态
            state.currentAtEdge = nextEdge.id
            state.edgeProgress = 0
            logger.debug("车辆 ${status.vehicleId} 进入边 ${nextEdge.id} 从节点 ${status.currentNodeId} 到 $nextNodeId")
        }

        state.currentSpeed = status.currentSpeed

        // 更新车辆在边上的进度
        updateVehicleEdgeProgress(status, state)

        // 更新电池状态
        state.batteryLife = status.batteryLife

        // 保存更新后的状态
        vehicleStates[status.vehicleId] = state
    }

    /**
     * 更新车辆在边上的进度
     */
    private fun updateVehicleEdgeProgress(status: VehicleStatus, state: DynamicVehicleState) {
        if (state.currentAtEdge != null) {
            // (用于计算进度百分比,首次进点的进度由 (速度 * 时间差构成)
            val edge = graph.getEdge(state.currentAtEdge!!) ?: return
            val speed = status.currentSpeed
            // 路长是毫米，所以要/1000
            val edgeLengthMm = edge.edgeLong.toDouble() / 1000.0
            if (edgeLengthMm <= 0) return
            val progressDeltaPercentage: Double
            val distanceTravelledMm: Double
            if (state.edgeProgress == 0) {
                val timeDeltaSeconds = (status.currentTime - status.passedNodeTime) / 1000.0
                if (timeDeltaSeconds <= 0) return

                distanceTravelledMm = speed * timeDeltaSeconds
            } else {
                // TODO 正式用的比例
                //distanceTravelledMm = speed * (status.currentTime - state.lastUpdateTime)
                distanceTravelledMm = speed
            }
            //计算增加的 百分比
            progressDeltaPercentage = (distanceTravelledMm / edgeLengthMm) * 100.0

            state.edgeProgress =
                (state.edgeProgress + progressDeltaPercentage).coerceAtMost(100.0).toInt()
            state.lastUpdateTime = System.currentTimeMillis()
        }
    }

    /**
     * 更新交通灯状态
     * @param status 交通灯状态
     */
    fun updateTrafficLightStatus(status: TrafficLightStatus) {
        graph.updateTrafficLight(status.lightId, status.status)
        logger.info("更新交通灯 ${status.lightId} 状态为 ${status.status}")
    }

    /**
     * 获取车辆下一步移动信息
     * @param vehicleId 车辆ID
     * @param currentNodeId 当前节点ID
     * @return 下一个节点ID和建议速度的Pair，如果无法确定则返回null
     */
    private fun getNextMove(vehicleId: String, currentNodeId: Int): Pair<Int, Double>? {
        val path = vehicleStates[vehicleId]?.plannedPath ?: return null
        val currentIndex = path.indexOf(currentNodeId)

        if (currentIndex == -1 || currentIndex + 1 >= path.size) {
            return null // 到达终点或(不在路径上,理论上来说，应该在路上)
        }

        val nextNodeId = path[currentIndex + 1]
        val edge = graph.getEdgesFrom(currentNodeId).find { it.rfidNodeEnd == nextNodeId } ?: return null

        // 检查路段容量
        if (graph.getVehiclesOnEdge(edge.id, vehicleId) >= edge.maxCar) {
            return handleEdgeCapacityExceeded(vehicleId, currentNodeId, path)
        }

        val recommendedSpeed = calculateRecommendedSpeed(vehicleId, edge)
        // 更新车辆速度
        // vehicleStates[vehicleId]?.currentSpeed = recommendedSpeed
        return nextNodeId to recommendedSpeed
    }

    /**
     * 处理边容量超出的情况
     */
    private fun handleEdgeCapacityExceeded(vehicleId: String, currentNodeId: Int, path: List<Int>): Pair<Int, Double>? {


        val dynamicVehicleState = vehicleStates[vehicleId]!!

        val (currentId, startNodeId) = determineVehicleCurrentNode(dynamicVehicleState)!!

        // 尝试重新规划路径
        val newPath = rePlanPath(vehicleId, currentId, startNodeId, path.last())
        if (newPath != null && newPath.size > 1) {
            val newNextNodeId = newPath[1]
            val newEdge = graph.getEdgesFrom(currentNodeId).find { it.rfidNodeEnd == newNextNodeId } ?: return null
            return newNextNodeId to calculateRecommendedSpeed(vehicleId, newEdge)
        } else {
            // 无法重规划，车辆需要等待
            return currentNodeId to 0.0
        }
    }

    /**
     * 计算基于 IDM 的建议加速度
     */
    private fun calcIDMAcceleration(
        current: DynamicVehicleState,
        front: DynamicVehicleState?,
        edge: Edge,
        params: IDMParams
    ): Double {
        val v = current.currentSpeed
        // desiredSpeed 不超过路段限速
        val v0 = params.desiredSpeed.coerceAtMost(edge.maxSpeed)
        val s0 = params.minGap
        val T = params.timeHeadway
        val a = params.maxAccel
        val b = params.comfortableDecel
        val δ = params.delta

        // 期望间距 s*
        val sStar = if (front != null) {
            val Δv = v - front.currentSpeed
            s0 + v * T + (v * Δv) / (2 * sqrt(a * b))
        } else {
            s0
        }

        // 实际间距 s（米）
        val myPos = current.edgeProgress / 100.0 * edge.edgeLong / 1000.0
        val frontPos = front?.let { it.edgeProgress / 100.0 * edge.edgeLong / 1000.0 } ?: (myPos + 1e6)
        val sActual = max(frontPos - myPos, 0.0)

        // IDM 加速度公式
        return a * (1 - (v / v0).pow(δ) - (sStar / sActual).pow(2))
    }

    /**
     * 计算并返回推荐速度：IDM + 加速度限制 + 平滑，无二级索引
     * - 如果 vehicleStates 中无该车辆记录，表示首次调用，返回期望速度（或其比例）。
     * - 如果找不到前车，视为自由路面加速。
     */
    private fun calculateRecommendedSpeed(
        vehicleId: String,
        edge: Edge,
        dt: Double = 1.0 // 仿真步长 (s)
    ): Double {
        // 构造 IDM 参数（desiredSpeed 可设为 edge.maxSpeed * 比例）
        val params = IDMParams(
            desiredSpeed = edge.maxSpeed * 0.97,  // 模拟略低于限速行驶
            minGap = 0.4,
            timeHeadway = 1.0,
            maxAccel = 0.4,
            comfortableDecel = 0.5
        )

        // 获取当前状态
        val current = vehicleStates[vehicleId]
            ?: // 首次调用，返回期望速度
            return params.desiredSpeed.coerceAtMost(edge.maxSpeed)

        // 查找同边前车（无前车时 front 为 null）
        val front = vehicleStates.values
            .asSequence()
            .filter { it.currentAtEdge == edge.id && it.vehicleId != vehicleId }
            .filter { it.edgeProgress > current.edgeProgress }
            .minByOrNull { it.edgeProgress }

        // 计算原始加速度
        val rawAccel = calcIDMAcceleration(current, front, edge, params)
        // 限制加/减速度
        val accel = rawAccel.coerceIn(-params.maxAccel, params.maxAccel)

        // 计算目标速度并限速
        val unconstrained = current.currentSpeed + accel * dt
        val targetSpeed = unconstrained.coerceIn(0.0, edge.maxSpeed)

        // 调整平滑系数以适应更大的 dt
        return current.currentSpeed * 0.2 + targetSpeed * 0.8
    }


    /**
     * 启用或禁用路段
     * @param edgeStatusUpdate 边状态更新信息
     */
    fun disableOrEnableEdge(edgeStatusUpdate: EdgeStatusUpdate): List<EdgeDisableReplanMessage>? {
        synchronized(GLOBAL_LOCK) {

            val edgeDisabled = graph.isEdgeDisabled(edgeStatusUpdate.edgeId)
            // 如果边的状态未改变，不需要修改路径
            if (edgeDisabled == edgeStatusUpdate.disabled) {
                return null
            }

            // 更新边的状态
            when (edgeStatusUpdate.disabled) {
                true -> graph.disableEdge(edgeStatusUpdate.edgeId)
                false -> graph.enableEdge(edgeStatusUpdate.edgeId)
            }

            //智能判断根据启用边和禁用边，智能选择路径规划逻辑
            val rePlannedVehicles = replanPathsForAffectedVehicles(edgeStatusUpdate.edgeId, edgeStatusUpdate.disabled)

            logger.info("边 ${edgeStatusUpdate.edgeId} ${if (edgeStatusUpdate.disabled) "已禁用" else "已启用"}")
            return rePlannedVehicles
        }
    }

    /**
     * 为受影响的车辆重新规划路径
     * @param edgeId 边ID
     * @param isDisable 是否禁用边
     */
    private fun replanPathsForAffectedVehicles(edgeId: Int, isDisable: Boolean): List<EdgeDisableReplanMessage> {

        val pathList = mutableListOf<EdgeDisableReplanMessage>()

        vehicleStates.forEach { (vehicleId, state) ->
            // 确定车辆当前的有效节点位置
            val (currentVehicleNodeId, startVehicleNodeId) = determineVehicleCurrentNode(state)
                ?: return@forEach

            // 找到车辆当前节点在规划路径中的索引
            val currentPathIndex = state.plannedPath.indexOf(startVehicleNodeId)
            if (currentPathIndex == -1) {
                logger.warn("车辆 $vehicleId 当前节点 $currentVehicleNodeId 不在其规划路径中")
                return@forEach
            }

            // 截取车辆尚未完成的路径部分
            val remainingPath = state.plannedPath.drop(currentPathIndex)

            // 如果是启用边，直接重规划
            if (!isDisable) {
                rePlanPath(vehicleId, currentVehicleNodeId, startVehicleNodeId, state.plannedPath.last())
                return@forEach
            }

            // 检查禁用的边是否影响车辆的路径
            checkAndReplanForDisabledEdge(
                vehicleId,
                edgeId,
                currentVehicleNodeId,
                startVehicleNodeId,
                remainingPath,
                state
            )?.let {
                pathList.add(
                    it
                )
            }
        }
        return pathList.toList()
    }

    /**
     * 确定车辆当前所在的节点
     * 返回一个 Pair<起始节点ID, 结束节点ID>，表示车辆当前所在的节点范围。
     * 如果车辆在边上，则返回 (边的起点, 边的终点)。
     * 如果车辆在节点上，则返回 (该节点, 该节点)。
     * 如果无法确定，则返回 null。
     */
    private fun determineVehicleCurrentNode(state: DynamicVehicleState): Pair<Int, Int>? {
        return when {
            // 如果车辆当前正在某条边上，其最近的有效节点是该边的起点和终点
            state.currentAtEdge != null -> {
                // 安全调用操作符 ?. 可以在结果为 null 时返回 null
                val edge = graph.getEdge(state.currentAtEdge!!)
                if (edge != null) {
                    // 确保 rfidNodeStart 和 rfidNodeEnd 是 Int 类型
                    Pair(edge.rfidNodeStart, edge.rfidNodeEnd)
                } else {
                    logger.warn("车辆 ${state.vehicleId} 位于边 ${state.currentAtEdge} 但该边不存在于图中。")
                    null // 边不存在，无法确定节点
                }
            }
            // 如果车辆当前停留在某个节点上
            graph.vehicleAtNodeMap.containsKey(state.vehicleId) -> {
                val nodeId = graph.vehicleAtNodeMap[state.vehicleId]
                // 如果能找到节点ID，则返回该节点作为起始和结束
                if (nodeId != null) {
                    Pair(nodeId, nodeId)
                } else {
                    logger.warn("车辆 ${state.vehicleId} 存在于 vehicleAtNodeMap 中但节点ID为 null。")
                    null
                }
            }
            // 如果无法确定精确位置，则从规划路径的第一个节点开始
            state.plannedPath.isNotEmpty() -> {
                val firstNode = state.plannedPath.first()
                // 规划路径的第一个节点作为起始和结束
                Pair(firstNode, firstNode)
            }
            // 无法确定任何有效位置
            else -> {
                logger.info("无法确定车辆 ${state.vehicleId} 的当前节点，所有条件均不满足。")
                null // 无法确定起点
            }
        }
    }

    /**
     * 检查并为禁用边重规划路径
     */
    private fun checkAndReplanForDisabledEdge(
        vehicleId: String,
        edgeId: Int,
        currentVehicleNodeId: Int,
        startVehicleNodeId: Int,
        remainingPath: List<Int>,
        state: DynamicVehicleState
    ): EdgeDisableReplanMessage? {
        var affected = false
        //重组路径
        var edgeDisableReplanMessage: EdgeDisableReplanMessage? = null


        // 遍历剩余路径，检查是否包含被禁用的边

        //graph.enableEdge(edgeId)
        for (i in 0 until remainingPath.size - 1) {
            val startNode = remainingPath[i]
            val endNode = remainingPath[i + 1]
            val edge = graph.getEdgesFrom(startNode).find { it.rfidNodeEnd == endNode }

            if (edge?.id == edgeId) {
                affected = true
                break
            }
        }
        //graph.disableEdge(edgeId)

        // 如果车辆的路径受到影响，则进行重规划
        if (affected) {
            // 旧的路径
            val plannedPath = vehicleStates[vehicleId]!!.plannedPath

            rePlanPath(vehicleId, currentVehicleNodeId, startVehicleNodeId, state.plannedPath.last())?.let {
                edgeDisableReplanMessage = EdgeDisableReplanMessage(
                    vehicleId,
                    state.plannedPath.last(),
                    plannedPath,
                    it
                )
            }
        }
        return edgeDisableReplanMessage
    }

    /**
     * 使用Dijkstra算法重新规划路径
     * @param vehicleId 车辆ID
     * @param currentNodeId 当前节点ID
     * @param startNodeId 规划起点节点ID
     * @param endNodeId 规划终点节点ID
     * @return 规划的路径，如果无法规划则返回null
     */
    private fun rePlanPath(vehicleId: String, currentNodeId: Int, startNodeId: Int, endNodeId: Int): List<Int>? {
        val result = pathfinder.findShortestPath(startNodeId, endNodeId)
        result?.path?.let {
            result.path = prependIfDifferent(it, currentNodeId)
            vehicleStates[vehicleId]?.plannedPath = result.path
            logger.info("为车辆 $vehicleId 重新规划路径: 从 $startNodeId 到 $endNodeId")
        }
        return result?.path
    }

    /**
     * 如果路径的第一个元素不等于指定元素，则在路径前添加该元素
     * @param path 路径
     * @param elem 要检查的元素
     * @return 处理后的路径
     */
    private fun prependIfDifferent(path: List<Int>, elem: Int): List<Int> {
        return if (path.firstOrNull() != elem) {
            listOf(elem) + path
        } else {
            path
        }
    }

    /**
     * 初始化地图
     * 从数据库加载节点和边数据
     */
    fun initMap() {
        try {
            val nodes = rfidMapper.selectAll()
            val edges = edgeMapper.selectAll()

            if (nodes.isEmpty() || edges.isEmpty()) {
                logger.warn("地图初始化数据不完整: 节点数=${nodes.size}, 边数=${edges.size}")
            }

            loadMap(nodes, edges)
            logger.info("地图初始化完成: 节点数=${nodes.size}, 边数=${edges.size}")
        } catch (e: Exception) {
            logger.error("地图初始化失败", e)
        }
    }

    /**
     * 获取车辆任务信息
     * @param pathRequest 路径请求
     * @return 车辆任务信息
     */
    fun getVehicleTaskInfo(pathRequest: PathRequest): VehicleTaskInfo {
        // 生成UUID和子topic
        val (taskId, taskTopic) = generateTaskInfo()

        val vehicleTaskInfo = VehicleTaskInfo(
            pathRequest.vehicleId,
            taskId,
            taskTopic
        )

        // 保存映射关系
        vehicleTaskMap[pathRequest.vehicleId] = vehicleTaskInfo
        taskTopicToVehicleIdMap[taskTopic] = pathRequest.vehicleId

        logger.info("为车辆 ${pathRequest.vehicleId} 创建任务: $taskId, topic: $taskTopic")
        return vehicleTaskInfo
    }

    /**
     * 生成任务ID和任务Topic
     * @return Pair<任务ID, 任务Topic>
     */
    private fun generateTaskInfo(): Pair<String, String> {
        val taskId = UUID.randomUUID().toString()  // 生成完整UUID
        val taskTopic = taskId.substringAfterLast("-")  // 截取最后一段作为Topic
        return taskId to taskTopic
    }

    /**
     * 验证任务Topic是否有效
     * @param taskTopic 任务Topic
     * @param vehicleId 车辆ID
     * @return 是否有效
     */
    fun isValidTaskTopic(taskTopic: String, vehicleId: String): Boolean {
        return taskTopicToVehicleIdMap[taskTopic] == vehicleId
    }

    /**
     * 获取车辆消息
     * @param requestPath 请求路径
     * @param pathRequest 路径请求
     * @param taskId 任务ID
     * @return 车辆消息
     */
    fun getVehicleMessage(requestPath: List<Int>?, pathRequest: PathRequest, taskId: String): VehicleMessage {
        // 标记车辆已初始化
        initializedVehicles.add(pathRequest.vehicleId)

        if (requestPath == null || requestPath.size < 2) {
            logger.warn("车辆 ${pathRequest.vehicleId} 的路径无效或太短")
            // 返回一个安全的默认消息
            return VehicleMessage(
                pathRequest.vehicleId,
                taskId,
                pathRequest.currentNodeId, // 无有效路径时保持在当前节点
                null,
                0.0,
                listOf(pathRequest.currentNodeId)
            )
        }

        val nextNodeId = requestPath[1]
        val edge = graph.findEdgeBetween(pathRequest.currentNodeId, nextNodeId)
        val speed = edge?.let { calculateRecommendedSpeed(pathRequest.vehicleId, it) } ?: 0.0

        return VehicleMessage(
            pathRequest.vehicleId,
            taskId,
            nextNodeId,
            edge?.navFavors,
            speed,
            requestPath
        )
    }

    /**
     * 检查车辆是否已初始化
     * @param vehicleId 车辆ID
     * @return 是否已初始化
     */
    private fun isVehicleReady(vehicleId: String): Boolean {
        return initializedVehicles.contains(vehicleId)
    }

    /**
     * 更新车辆状态并返回任务结果
     * @param vehicleState 车辆状态
     * @return 任务结果
     */
    fun updateVehicle(vehicleState: VehicleStatus): TaskResult {
        // 检查车辆是否已初始化
        if (!isVehicleReady(vehicleState.vehicleId)) {
            logger.debug("车辆 ${vehicleState.vehicleId} 未初始化，忽略状态更新")
            return TaskResult.NotFound
        }

        // 获取车辆任务信息
        val vehicleTaskInfo = vehicleTaskMap[vehicleState.vehicleId] ?: run {
            logger.warn("找不到车辆 ${vehicleState.vehicleId} 的任务信息")
            return TaskResult.NotFound
        }

        // 更新车辆状态
        updateVehicleStatus(vehicleState)

        // 获取车辆当前状态
        val vehicleProgress = getVehicleProgress(vehicleState.vehicleId) ?: run {
            logger.warn("找不到车辆 ${vehicleState.vehicleId} 的状态信息")
            return TaskResult.NotFound
        }

        // 判断车辆是否已到达终点
        if (vehicleProgress.currentAtEdge == null && vehicleProgress.edgeProgress == 100) {
            logger.info("车辆 ${vehicleState.vehicleId} 已完成任务 ${vehicleTaskInfo.taskId}")
            return createSuccessEndResult(vehicleState, vehicleTaskInfo, vehicleProgress)
        }

        // 获取下一步移动信息
        val nextMove = getNextMove(vehicleState.vehicleId, vehicleState.currentNodeId)
        return createSuccessResult(vehicleState, vehicleTaskInfo, vehicleProgress, nextMove)
    }

    /**
     * 创建成功结束的任务结果
     */
    private fun createSuccessEndResult(
        vehicleState: VehicleStatus,
        vehicleTaskInfo: VehicleTaskInfo,
        vehicleProgress: DynamicVehicleState
    ): TaskResult.SuccessEnd {
        val vehicleEndMessage = VehicleEndMessage(
            vehicleState.vehicleId,
            vehicleTaskInfo.taskId,
            vehicleProgress.plannedPath.last()
        )
        return TaskResult.SuccessEnd(vehicleEndMessage, vehicleTaskInfo.taskTopic)
    }

    /**
     * 创建成功的任务结果
     */
    private fun createSuccessResult(
        vehicleState: VehicleStatus,
        vehicleTaskInfo: VehicleTaskInfo,
        vehicleProgress: DynamicVehicleState,
        nextMove: Pair<Int, Double>?
    ): TaskResult.Success {
        val (nextNodeId, speed) = nextMove ?: (null to 0.0)
        val edge = nextNodeId?.let { graph.findEdgeBetween(vehicleState.currentNodeId, it) }

        val vehicleMessage = VehicleMessage(
            vehicleState.vehicleId,
            vehicleTaskInfo.taskId,
            nextNodeId,
            edge?.navFavors,
            speed,
            vehicleProgress.plannedPath
        )
        return TaskResult.Success(vehicleMessage, vehicleTaskInfo.taskTopic)
    }

    fun getMapInfo(): MapMessage {
        val (nodesList, edgesList) = graph.getEdgesAndNodes()
        return MapMessage(nodesList, edgesList)
    }
}